#include "broker.h"

#include <stdio.h>
#include <stdint.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>

struct bcshm_t shm;
struct bcshmseg_t shmseg;

inline int MIN(int a, int b) { return a > b ? b : a; }

void reader(){
  
  while(1){
    bcshm_read(SHM_FLIGHT_TASK, &shm, &shmseg.flight_task);
    bcshm_read(SHM_FCACTION, &shm, &shmseg.fc_action);
    bcshm_read(SHM_EVENT, &shm, &shmseg.event);
    bcshm_read(SHM_FC_TLM, &shm, &shmseg.fc_tlm);

    printf("Flight task:\n");
    printf("\t Hight: %d\n", (int)shmseg.flight_task.hight);
    printf("\tStart time: %d\n", (int)shmseg.flight_task.start_time);
    printf("\tHang time: %d\n", (int)shmseg.flight_task.hang_time);

    printf("\tFC:\n");
    printf("\t\tFlight mode:\t%d\n", (int)shmseg.fc_tlm.flight_mode);
    printf("\t\tGps fix:\t%d\n", (int)shmseg.fc_tlm.gps_fix);
    printf("\t\tActs count:\t%d\n", (int)shmseg.fc_tlm.actuators_count);
    for(int i = 0; i < ((int)shmseg.fc_tlm.actuators_count < 8 ? (int)shmseg.fc_tlm.actuators_count : 8) ; ++i)
      printf("\t\t\tmotor %d:\t%d\n", i, (int)shmseg.fc_tlm.actuators[i]);
    printf("\t\tHight:\t%d\n", (int)shmseg.fc_tlm.hight);
    printf("\t\tVelocity:\t%d\n", (int)shmseg.fc_tlm.speed);
    
    printf("Action (FC): %d\n", (int)shmseg.fc_action);

    printf("Current event: %d\n", (int)shmseg.event);
    printf("*******************************************************\n");

    usleep(1e6);
  }
}

int main(int argc, char* argv[]) {

  bcshm_delete();
  bcshm_init();
  bcshm_open(&shm);

  //EVENT_t ev = EVENT_UNKNOWN;
  //bcshm_write(SHM_EVENT, &shm, &ev);
  
  reader();
  
  bcshm_close(&shm);
  return 0;
}
